/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package org.firestorm.traficforecast.astar;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;

import org.firestorm.traficforecast.database.MyNode;
import org.firestorm.traficforecast.database.Road;

/**
 *
 * @author HMT
 */
public class FindWay {

    private ArrayList<Way> fringe;//Chá»©a cÃ¡c cung Ä‘Æ°á»�ng sáº½ Ä‘Æ°á»£c xÃ©t
    private ArrayList<String> ids;//Chá»©a id cung Ä‘Æ°á»�ng Ä‘Ã£ xÃ©t

    public FindWay() {
        fringe = new ArrayList<Way>();
        ids = new ArrayList<String>();
    }

    public ArrayList<Way> getFringe() {
        return fringe;
    }

    public void setFringe(ArrayList<Way> fringe) {
        this.fringe = fringe;
    }

    //HÃ m tÃ­nh khoáº£ng cÃ¡ch ngáº¯n nháº¥t giá»¯a 2 Ä‘iá»ƒm trÃªn máº·t cáº§u
    public static double distance(MyNode a, MyNode b) {
        double lat1 = a.getLat() * Math.PI / 180;
        double lat2 = b.getLat() * Math.PI / 180;
        double deltaLat = Math.abs(lat1 - lat2);
        double deltaLon = Math.abs((a.getLon() - b.getLon())) * Math.PI / 180;
        double a_ = Math.sin(deltaLat / 2) * Math.sin(deltaLat / 2)
                + Math.cos(lat1) * Math.cos(lat2) * Math.sin(deltaLon / 2)
                * Math.sin(deltaLon / 2);
        double c_ = 2 * Math.atan2(Math.sqrt(a_), Math.sqrt(1 - a_));
        double d = 6371009 * c_;
        return d;
    }

    //Khá»Ÿi táº¡o tráº¡ng thÃ¡i ban Ä‘áº§u cho cáº¥u trÃºc fringe
    public void makeWay(Road a, Road b) {
        fringe.add(new Way(a, null, a.getLenEstimate(), distance(a.getFinish(), b.getStart())));
    }

    //Kiá»ƒm tra Ä‘Ã£ tá»›i Ä‘Ã­ch hay chÆ°a
    public boolean goalTest(Road a, Road b) {
        boolean t = false;
        if (a.getFinish().getId().equals(b.getStart().getId())) {
            t = true;
        }
        return t;
    }

    //Tráº£ vá»� danh sÃ¡ch Ä‘Æ°á»�ng tá»« Ä‘áº§u tá»›i Ä‘Ã­ch
    public ArrayList<Road> solution(Way a, Road b) {
        ArrayList<Road> temp = new ArrayList<Road>();
        while (a.getParent() != null) {
            temp.add(0, a.getRoad());
            a = a.getParent();
        }
        temp.add(0, a.getRoad());
        temp.add(b);
        return temp;
    }

    //Táº¡o danh sÃ¡ch Way káº¿ tiáº¿p
    public void expand(Way a, Road b) {
        ArrayList<Way> temp = new ArrayList<Way>();
        ArrayList<Road> nextRoad = a.getRoad().getNextRoad(fringe, ids);
        for (int i = 0; i < nextRoad.size(); i++) {
            Way temp1 = new Way();
            temp1.setParent(a);
            temp1.setRoad(nextRoad.get(i));
            temp1.setCost(a.getCost() + nextRoad.get(i).getLenEstimate());
            temp1.setCostEstimate(distance(nextRoad.get(i).getFinish(), b.getStart()));
            temp.add(temp1);
        }
        insertAll(temp);
    }

    //Há»£p hai ArrayList<Way>
    public void insertAll(ArrayList<Way> a) {
        if (!a.isEmpty()) {
            sortCost(a);
            if (fringe.isEmpty()) {
                fringe = a;
            } else {
                int j = 0;
                for (int i = 0; i < a.size(); i++) {
                    double cost1 = fringe.get(j).getCost() + fringe.get(j).getCostEstimate();
                    double cost2 = a.get(i).getCost() + a.get(i).getCostEstimate();
                    if (cost2 <= cost1) {
                        fringe.add(j, a.get(i));
                        j++;
                    } else {
                        while ((j < fringe.size())
                                && ((fringe.get(j).getCost() + fringe.get(j).getCostEstimate()) < cost2)) {
                            j++;
                        }
                        if (j != fringe.size()) {
                            i--;
                        } else {
                            for (; i < a.size(); i++) {
                                fringe.add(a.get(i));
                            }
                        }
                    }
                }
            }
        }
    }

    public ArrayList<Road> aStar(Road a, Road b) {
        ArrayList<Road> result = new ArrayList<Road>();
        makeWay(a, b);
        while (!fringe.isEmpty()) {
            Way temp = fringe.remove(0);
            ids.add(temp.getRoad().getId());
            if (goalTest(temp.getRoad(), b)) {
                result = solution(temp, b);
                break;
            } else {
                expand(temp, b);
            }
        }
        return result;
    }

    //Sáº¯p xáº¿p theo chi phÃ­ hiá»‡n táº¡i vÃ  chi phÃ­ Æ°á»›c lÆ°á»£ng
    public static void sortCost(ArrayList<Way> temp) {
        Collections.sort(temp, new Comparator<Way>() {
            public int compare(Way way1, Way way2) {
                double cost1 = way1.getCost() + way1.getCostEstimate();
                double cost2 = way2.getCost() + way2.getCostEstimate();
                if (cost1 > cost2) {
                    return 1;
                } else {
                    if (cost1 == cost2) {
                        return 0;
                    } else {
                        return -1;
                    }
                }
            }
        });
    }
}
